﻿using System;
using System.Diagnostics;
using System.Threading;
using System.Threading.Tasks;

namespace AMEE.BeinControl
{
    /// <summary>
    /// 4-Legs Controller Instance to perfom walking.
    /// </summary>
    /// <remarks>
    /// Optionale Änderung:
    ///   Bein StateChanged event nutzen, um allgemeine Bereitschaft hier in einer Egenschaft festzuhalten.
    /// </remarks>
    public class Stepper
    {
        #region Constants

        internal const String VL_IP = "192.168.22.1";
        internal const int VL_Port = 42;
        internal const String VR_IP = "192.168.22.1";
        internal const int VR_Port = 42;
        internal const String HL_IP = "192.168.22.1";
        internal const int HL_Port = 42;
        internal const String HR_IP = "192.168.22.1";
        internal const int HR_Port = 42;
        internal static readonly String[] IPs = { VL_IP, VR_IP, HL_IP, HR_IP};
        internal static readonly int[] Ports = { VL_Port, VR_Port, HL_Port, HR_Port };

        internal static readonly int[] VL_Down = { 1045, 1319,  870,  730 };
        internal static readonly int[] VL_Up =   { 1045, 1390, 1027,  795 };

        internal static readonly int[] VR_Down = { 1045, 1319, 870, 730 };
        internal static readonly int[] VR_Up = { 1045, 1390, 1027, 795 };

        internal static readonly int[] HL_Down = { 1045, 1319, 870, 730 };
        internal static readonly int[] HL_Up = { 1045, 1390, 1027, 795 };

        internal static readonly int[] HR_Down = { 1045, 1319, 870, 730 };
        internal static readonly int[] HR_Up = { 1045, 1390, 1027, 795 };
        internal static readonly int[][] CommandParams = { VL_Down, VL_Up, VR_Down, VR_Up, HL_Down, HL_Up, HR_Down, HR_Up, };

        #endregion

        #region Fields
        /// <summary>VL, VR, HL, HR</summary>
        Bein[] _legs;
        #endregion

        #region Properties

        /// <summary>
        /// Gets a value indicating whether this <see cref="Stepper"/> is ready to perfom Actions.
        /// </summary>
        /// <value>
        ///   <c>true</c> if ready; otherwise, <c>false</c>.
        /// </value>
        public bool Ready { get { return this.areLegsReady(); } }

        internal Bein VL { get { return _legs[0]; } }

        internal Bein VR { get { return _legs[1]; } }

        internal Bein HL { get { return _legs[2]; } }

        internal Bein HR { get { return _legs[3]; } }

        #endregion

        #region Constructor
        /// <summary>
        /// Initializes a new instance of the <see cref="Stepper"/> class.
        /// </summary>
        [Obsolete("Use Stepper(Bein, Bein, Bein, Bein).")]
        public Stepper() {
            //initLegs();
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="Stepper"/> class.
        /// </summary>
        /// <param name="frontLeft">The front left leg instance.</param>
        /// <param name="frontright">The frontright leg instance.</param>
        /// <param name="backLeft">The back left leg instance.</param>
        /// <param name="backRight">The back right leg instance.</param>
        /// <exception cref="System.ArgumentNullException"></exception>
        public Stepper(Bein frontLeft, Bein frontright, Bein backLeft, Bein backRight) {
            if (null == frontLeft)
                throw new ArgumentNullException("frontLeft");
            if (null == frontright)
                throw new ArgumentNullException("frontright");
            if (null == backLeft)
                throw new ArgumentNullException("backLeft");
            if (null == backRight)
                throw new ArgumentNullException("backRight");
            _legs = new Bein[] { frontLeft, frontright, backLeft, backRight };
        }

        #endregion

        #region Public

        /// <summary>
        /// Echoes the specified leg ID.
        /// </summary>
        /// <param name="legID">The leg ID.</param>
        public void Echo(int legID) {
            if (legID < 1 || legID > 4)
                Debug.WriteLine(String.Format("Stepper.Echo({0})", legID), "Warning Stepper");
            _legs[legID - 1].Echo();
        }

        /// <summary>
        /// Let the Robot take the starting pose for Fast Walk (blocking)<para>
        /// Methode blockiert bis Zyklus beendet oder ein
        /// Zwischenbefehl nicht nach 15 Sekunden beendet ist (Bein-Status = Done).
        /// </para>
        /// </summary>
        public void PerformWalkFastBase() {
            if (!areLegsReady()) {
                Debug.WriteLine("Stepper.PerformStepCyclus - not ready to start", "Warning Stepper");
                return;
            }
            // vorne links + hinten rechts vor
            VL.Do(BeinTestCommands.Step);
            HR.Do(BeinTestCommands.Step);
            // VR + HL zurück
            VR.Do(BeinTestCommands.Pull);
            HL.Do(BeinTestCommands.Pull);
            // Polling each half second
            int cnt = 0;
            do {
                Thread.Sleep(500);
                cnt++;
                //ready = areLegsReady();
            } while (cnt < 30 && !areLegsReady());
            if (cnt >= 30) {
                Debug.WriteLine("Stepper.PerformWalkFastBase - no Feedback after 15 sec.", "Warning Stepper");
                return;
            }
        }

        /// <summary>
        /// Performs the step cyclus (blocking)<para>
        /// <b>ACHTUNG!</b> Vorher erst PerformWalkFastBase() ausführen!</para><para>
        /// Je 2 Beine diagonal hoch vor und runter. Methode blockiert bis Zyklus beendet oder ein
        /// Zwischenbefehl nicht nach 15 Sekunden beendet ist (Bein-Status = Done).
        /// </para>
        /// </summary>
        public void PerformStepCyclus() {
            if (!areLegsReady()) {
                Debug.WriteLine("Stepper.PerformStepCyclus - not ready to start", "Warning Stepper");
                return;
            }
            int millis = 0;
            int millisMax = 15000;
            int millisOffset = 250;

            // Grundposition vorausgesetzt: VL+HR auf DownAhead (Step)
            // Phase 1: VR+HL heben
            VR.Do(BeinTestCommands.Raise);
            HL.Do(BeinTestCommands.Raise);
            // Polling
            do {
                Thread.Sleep(millisOffset);
                millis += millisOffset;
            } while (millis < millisMax && !areLegsReady());
            if (millis >= millisMax) {
                Debug.WriteLine("Stepper.PerformStepCyclus - no Feedback after "+ (millisMax / 1000) +" sec.", "Warning Stepper");
                return;
            }
            millis = 0;

            // Phase 2: VL+HR ziehen & VR+HL vor greifen
            VL.Do(BeinTestCommands.Pull);
            HR.Do(BeinTestCommands.Pull);
            VR.Do(BeinTestCommands.GrabFront);
            HL.Do(BeinTestCommands.GrabFront);
            // Polling
            do {
                Thread.Sleep(millisOffset);
                millis += millisOffset;
            } while (millis < millisMax && !areLegsReady());
            if (millis >= millisMax) {
                Debug.WriteLine("Stepper.PerformStepCyclus - no Feedback after " + (millisMax / 1000) + " sec.", "Warning Stepper");
                return;
            }
            millis = 0;

            // Phase 3: VR+HL vorne aufsetzen (Step)
            VR.Do(BeinTestCommands.Step);
            HL.Do(BeinTestCommands.Step);
            // Polling
            do {
                Thread.Sleep(millisOffset);
                millis += millisOffset;
            } while (millis < millisMax && !areLegsReady());
            if (millis >= millisMax) {
                Debug.WriteLine("Stepper.PerformStepCyclus - no Feedback after " + (millisMax / 1000) + " sec.", "Warning Stepper");
                return;
            }
            millis = 0;

            // Phase 4: VL+HR heben
            VL.Do(BeinTestCommands.Raise);
            HR.Do(BeinTestCommands.Raise);
            // Polling
            do {
                Thread.Sleep(millisOffset);
                millis += millisOffset;
            } while (millis < millisMax && !areLegsReady());
            if (millis >= millisMax) {
                Debug.WriteLine("Stepper.PerformStepCyclus - no Feedback after " + (millisMax / 1000) + " sec.", "Warning Stepper");
                return;
            }
            millis = 0;

            // Phase 5: VL+HR vor greifen & VR+HL ziehen
            VL.Do(BeinTestCommands.GrabFront);
            HR.Do(BeinTestCommands.GrabFront);
            VR.Do(BeinTestCommands.Pull);
            HL.Do(BeinTestCommands.Pull);
            // Polling
            do {
                Thread.Sleep(millisOffset);
                millis += millisOffset;
            } while (millis < millisMax && !areLegsReady());
            if (millis >= millisMax) {
                Debug.WriteLine("Stepper.PerformStepCyclus - no Feedback after " + (millisMax / 1000) + " sec.", "Warning Stepper");
                return;
            }
            millis = 0;

            // Phase 6: VL+HR vorne aufsetzen (Step)
            VL.Do(BeinTestCommands.Step);
            HR.Do(BeinTestCommands.Step);
            // Polling
            do {
                Thread.Sleep(millisOffset);
                millis += millisOffset;
            } while (millis < millisMax && !areLegsReady());
            if (millis >= millisMax) {
                Debug.WriteLine("Stepper.PerformStepCyclus - no Feedback after " + (millisMax / 1000) + " sec.", "Warning Stepper");
                return;
            }
            millis = 0;

            // Phase 7 = fertig (Grundposition)
        }

        /// <summary>
        /// Performs the stamp cyclus (blocking)<para>
        /// <b>ACHTUNG!</b> Vorher erst PerformWalkFastBase() ausführen!</para><para>
        /// Je 2 Beine diagonal hoch und runter. Methode blockiert bis Zyklus beendet oder ein
        /// Zwischenbefehl nicht nach 15 Sekunden beendet ist (Bein-Status = Done).
        /// </para>
        /// </summary>
        public void PerformStampCyclus() {
            if (!areLegsReady()) {
                Debug.WriteLine("Stepper.PerformStampCyclus - not ready to start", "Warning Stepper");
                return;
            }
            int millis = 0;
            int millisMax = 15000;
            int millisOffset = 250;

            // Grundposition vorausgesetzt: VL+HR auf DownAhead (Step) & VR+HL auf DownBack (Pull)
            // Phase 1: VL+HR auf UpAhead (GrabFront)
            VL.Do(BeinTestCommands.GrabFront);
            HR.Do(BeinTestCommands.GrabFront);
            // Polling
            do {
                Thread.Sleep(millisOffset);
                millis += millisOffset;
            } while (millis < millisMax && !areLegsReady());
            if (millis >= millisMax) {
                Debug.WriteLine("Stepper.PerformStepCyclus - no Feedback after " + (millisMax / 1000) + " sec.", "Warning Stepper");
                return;
            }
            millis = 0;

            // Phase 2: VL+HR auf DownAhead (Step)
            VL.Do(BeinTestCommands.Step);
            HR.Do(BeinTestCommands.Step);
            // Polling
            do {
                Thread.Sleep(millisOffset);
                millis += millisOffset;
            } while (millis < millisMax && !areLegsReady());
            if (millis >= millisMax) {
                Debug.WriteLine("Stepper.PerformStepCyclus - no Feedback after " + (millisMax / 1000) + " sec.", "Warning Stepper");
                return;
            }
            millis = 0;

            // Phase 3: VR+HL au Up (Raise)
            VR.Do(BeinTestCommands.Raise);
            HL.Do(BeinTestCommands.Raise);
            // Polling
            do {
                Thread.Sleep(millisOffset);
                millis += millisOffset;
            } while (millis < millisMax && !areLegsReady());
            if (millis >= millisMax) {
                Debug.WriteLine("Stepper.PerformStepCyclus - no Feedback after " + (millisMax / 1000) + " sec.", "Warning Stepper");
                return;
            }
            millis = 0;

            // Phase 4: VR+HL auf DownBack (Pull)
            VR.Do(BeinTestCommands.Pull);
            HL.Do(BeinTestCommands.Pull);
            // Polling
            do {
                Thread.Sleep(millisOffset);
                millis += millisOffset;
            } while (millis < millisMax && !areLegsReady());
            if (millis >= millisMax) {
                Debug.WriteLine("Stepper.PerformStepCyclus - no Feedback after " + (millisMax / 1000) + " sec.", "Warning Stepper");
                return;
            }
            millis = 0;

        }

        /// <summary>
        /// Goes into the park position (blocking).
        /// </summary>
        public void GoIntoParkPosition() {
            if (!areLegsReady()) {
                Debug.WriteLine("Stepper.GoIntoParkPosition - not ready to start", "Warning Stepper");
                return;
            }
            int cnt = 0;
            //bool ready = false;
            VL.Do(BeinTestCommands.Park);
            VR.Do(BeinTestCommands.Park);
            HL.Do(BeinTestCommands.Park);
            HR.Do(BeinTestCommands.Park);
            // Polling each second
            do {
                Thread.Sleep(1000);
                cnt++;
                //ready = areLegsReady();
            } while (cnt < 15 && !areLegsReady());
            if (cnt >= 15) {
                Debug.WriteLine("Stepper.GoIntoParkPosition - no Feedback after 15 sec.", "Warning Stepper");
                return;
            }
        }

        /// <summary>
        /// Shuts down. Beendet die Kommunikation mit der Hardware (schließt Sockets, Beine,...)
        /// </summary>
        public void ShutDown() {
            foreach (Bein bein in _legs) {
                bein.Dispose();
            }
        }

        #endregion


        #region Helper / Private

        //void initLegs() {
        //    _legs = new Bein[4];
        //    for (int i = 0; i < _legs.Length; i++) {
        //        _legs[i] = new Bein(Stepper.IPs[i], Stepper.Ports[i]);
        //        int[] dnParas = Stepper.CommandParams[2 * i];
        //        int[] upParas = Stepper.CommandParams[(2 * i) + 1];

        //        // neue Paramet setzen

        //        //_legs[i].SetDownAheadParameters(dnParas[0], dnParas[1], dnParas[2]);
        //        //_legs[i].SetUpParameters(upParas[0], upParas[1], upParas[2]);
        //    }
        //}

        /// <summary>
        /// Ares the legs ready.
        /// </summary>
        /// <returns>true, if no leg is working. false otherwise</returns>
        bool areLegsReady() {
            if (VL.State == BeinState.Done || VL.State == BeinState.Initialized) {
                if (VR.State == BeinState.Done || VR.State == BeinState.Initialized) {
                    if (HL.State == BeinState.Done || HL.State == BeinState.Initialized) {
                        if (HR.State == BeinState.Done || HR.State == BeinState.Initialized) {
                            return true;
                        }
                    }
                }
            }
            return false;
        }

        #endregion

    }
}
